//
// Created by cyq on 2022/12/9.
//
#include "math.h"
#include "math_basic.h"

float AmplitudeLimit(float x, float threshold)
{
    if(x > threshold)
        return threshold;
    if(x < -threshold)
        return -threshold;
    return x;
}

float RotationSoluteX(float deltaDegreeNishi, float GimbalSpeedX, float GimbalSpeedY)
{
    //deltaDegreeNishi是0-1圈的yaw电机逆时针旋转角度
    float cost = cosf(2.f * PI * deltaDegreeNishi);
    float sint = sinf(2.f * PI * deltaDegreeNishi);

    return GimbalSpeedX * cost + GimbalSpeedY * sint;
}

float RotationSoluteY(float deltaDegreeNishi, float GimbalSpeedX, float GimbalSpeedY)
{
    //deltaDegreeNishi是0-1圈的yaw电机逆时针旋转角度
    float cost = cosf(2.f * PI * deltaDegreeNishi);
    float sint = sinf(2.f * PI * deltaDegreeNishi);

    return GimbalSpeedY * cost - GimbalSpeedX * sint;
}
